home *** CD-ROM | disk | FTP | other *** search
/ OpenGL Superbible (2nd Edition) / OpenGL SuperBible e2.iso / tools / GLUT-3.7 / PROGS / DEMOS / YACME / invertmat.c < prev    next >
Encoding:
C/C++ Source or Header  |  1998-08-12  |  4.9 KB  |  213 lines
  1. /*
  2.  *    matrix -
  3.  *        Some utilities for working with matricies.
  4.  *
  5.  *                Paul Haeberli - 1985
  6.  */
  7. #include <stdio.h>
  8.  
  9. void
  10. invertmat(float from[4][4], float to[4][4])
  11. {
  12.     float wtemp[4][8];
  13.     float m0,m1,m2,m3,s;
  14.     float *r0,*r1,*r2,*r3, *rtemp;
  15.  
  16.     r0 = wtemp[0];
  17.     r1 = wtemp[1];
  18.     r2 = wtemp[2];
  19.     r3 = wtemp[3];
  20.     r0[0] = from[0][0];        /* build up [A][I]    */
  21.     r0[1] = from[0][1];
  22.     r0[2] = from[0][2];
  23.     r0[3] = from[0][3];
  24.     r0[4] = 1.0;
  25.     r0[5] = 0.0;
  26.     r0[6] = 0.0;
  27.     r0[7] = 0.0;
  28.     r1[0] = from[1][0];
  29.     r1[1] = from[1][1];
  30.     r1[2] = from[1][2];
  31.     r1[3] = from[1][3];
  32.     r1[4] = 0.0;
  33.     r1[5] = 1.0;
  34.     r1[6] = 0.0;
  35.     r1[7] = 0.0;
  36.     r2[0] = from[2][0];
  37.     r2[1] = from[2][1];
  38.     r2[2] = from[2][2];
  39.     r2[3] = from[2][3];
  40.     r2[4] = 0.0;
  41.     r2[5] = 0.0;
  42.     r2[6] = 1.0;
  43.     r2[7] = 0.0;
  44.     r3[0] = from[3][0];
  45.     r3[1] = from[3][1];
  46.     r3[2] = from[3][2];
  47.     r3[3] = from[3][3];
  48.     r3[4] = 0.0;
  49.     r3[5] = 0.0;
  50.     r3[6] = 0.0;
  51.     r3[7] = 1.0;
  52.  
  53.     if (r0[0] == 0.0) {        /* swap rows if needed        */
  54.     if (r1[0] == 0.0) {
  55.         if (r2[0] == 0.0) {
  56.         if (r3[0] == 0.0) goto singular;
  57.         rtemp = r0; r0 = r3; r3 = rtemp;
  58.         }
  59.         else {rtemp = r0; r0 = r2; r2 = rtemp;}
  60.     }
  61.     else {rtemp = r0; r0 = r1; r1 = rtemp;}
  62.     }
  63.     m1 = r1[0]/r0[0];        /* eliminate first variable    */
  64.     m2 = r2[0]/r0[0];
  65.     m3 = r3[0]/r0[0];
  66.     s = r0[1];
  67.     r1[1] = r1[1] - m1 * s;
  68.     r2[1] = r2[1] - m2 * s;
  69.     r3[1] = r3[1] - m3 * s;
  70.     s = r0[2];
  71.     r1[2] = r1[2] - m1 * s;
  72.     r2[2] = r2[2] - m2 * s;
  73.     r3[2] = r3[2] - m3 * s;
  74.     s = r0[3];
  75.     r1[3] = r1[3] - m1 * s;
  76.     r2[3] = r2[3] - m2 * s;
  77.     r3[3] = r3[3] - m3 * s;
  78.     s = r0[4];
  79.     if (s != 0.0) {
  80.     r1[4] = r1[4] - m1 * s;
  81.     r2[4] = r2[4] - m2 * s;
  82.     r3[4] = r3[4] - m3 * s;
  83.     }
  84.     s = r0[5];
  85.     if (s != 0.0) {
  86.     r1[5] = r1[5] - m1 * s;
  87.     r2[5] = r2[5] - m2 * s;
  88.     r3[5] = r3[5] - m3 * s;
  89.     }
  90.     s = r0[6];
  91.     if (s != 0.0) {
  92.     r1[6] = r1[6] - m1 * s;
  93.     r2[6] = r2[6] - m2 * s;
  94.     r3[6] = r3[6] - m3 * s;
  95.     }
  96.     s = r0[7];
  97.     if (s != 0.0) {
  98.     r1[7] = r1[7] - m1 * s;
  99.     r2[7] = r2[7] - m2 * s;
  100.     r3[7] = r3[7] - m3 * s;
  101.     }
  102.  
  103.     if (r1[1] == 0.0) {        /* swap rows if needed        */
  104.     if (r2[1] == 0.0) {
  105.         if (r3[1] == 0.0) goto singular;
  106.         rtemp = r1; r1 = r3; r3 = rtemp;
  107.     }
  108.     else {rtemp = r1; r1 = r2; r2 = rtemp;}
  109.     }
  110.     m2 = r2[1]/r1[1];        /* eliminate second variable    */
  111.     m3 = r3[1]/r1[1];
  112.     r2[2] = r2[2] - m2 * r1[2];
  113.     r3[2] = r3[2] - m3 * r1[2];
  114.     r3[3] = r3[3] - m3 * r1[3];
  115.     r2[3] = r2[3] - m2 * r1[3];
  116.     s = r1[4];
  117.     if (s != 0.0) {
  118.     r2[4] = r2[4] - m2 * s;
  119.     r3[4] = r3[4] - m3 * s;
  120.     }
  121.     s = r1[5];
  122.     if (s != 0.0) {
  123.     r2[5] = r2[5] - m2 * s;
  124.     r3[5] = r3[5] - m3 * s;
  125.     }
  126.     s = r1[6];
  127.     if (s != 0.0) {
  128.     r2[6] = r2[6] - m2 * s;
  129.     r3[6] = r3[6] - m3 * s;
  130.     }
  131.     s = r1[7];
  132.     if (s != 0.0) {
  133.     r2[7] = r2[7] - m2 * s;
  134.     r3[7] = r3[7] - m3 * s;
  135.     }
  136.  
  137.     if (r2[2] == 0.0) {        /* swap last 2 rows if needed    */
  138.     if (r3[2] == 0.0) goto singular;
  139.     rtemp = r2; r2 = r3; r3 = rtemp;
  140.     }
  141.     m3 = r3[2]/r2[2];        /* eliminate third variable    */
  142.     r3[3] = r3[3] - m3 * r2[3];
  143.     r3[4] = r3[4] - m3 * r2[4];
  144.     r3[5] = r3[5] - m3 * r2[5];
  145.     r3[6] = r3[6] - m3 * r2[6];
  146.     r3[7] = r3[7] - m3 * r2[7];
  147.  
  148.     if (r3[3] == 0.0) goto singular;
  149.     s = 1.0f/r3[3];        /* now back substitute row 3    */
  150.     r3[4] = r3[4] * s;
  151.     r3[5] = r3[5] * s;
  152.     r3[6] = r3[6] * s;
  153.     r3[7] = r3[7] * s;
  154.  
  155.     m2 = r2[3];            /* now back substitute row 2    */
  156.     s = 1.0f/r2[2];
  157.     r2[4] = s * (r2[4] - r3[4] * m2);
  158.     r2[5] = s * (r2[5] - r3[5] * m2);
  159.     r2[6] = s * (r2[6] - r3[6] * m2);
  160.     r2[7] = s * (r2[7] - r3[7] * m2);
  161.     m1 = r1[3];
  162.     r1[4] = (r1[4] - r3[4] * m1);
  163.     r1[5] = (r1[5] - r3[5] * m1);
  164.     r1[6] = (r1[6] - r3[6] * m1);
  165.     r1[7] = (r1[7] - r3[7] * m1);
  166.     m0 = r0[3];
  167.     r0[4] = (r0[4] - r3[4] * m0);
  168.     r0[5] = (r0[5] - r3[5] * m0);
  169.     r0[6] = (r0[6] - r3[6] * m0);
  170.     r0[7] = (r0[7] - r3[7] * m0);
  171.  
  172.     m1 = r1[2];            /* now back substitute row 1    */
  173.     s = 1.0f/r1[1];
  174.     r1[4] = s * (r1[4] - r2[4] * m1);
  175.     r1[5] = s * (r1[5] - r2[5] * m1);
  176.     r1[6] = s * (r1[6] - r2[6] * m1);
  177.     r1[7] = s * (r1[7] - r2[7] * m1);
  178.     m0 = r0[2];
  179.     r0[4] = (r0[4] - r2[4] * m0);
  180.     r0[5] = (r0[5] - r2[5] * m0);
  181.     r0[6] = (r0[6] - r2[6] * m0);
  182.     r0[7] = (r0[7] - r2[7] * m0);
  183.  
  184.     m0 = r0[1];            /* now back substitute row 0    */
  185.     s = 1.0f/r0[0];
  186.     r0[4] = s * (r0[4] - r1[4] * m0);
  187.     r0[5] = s * (r0[5] - r1[5] * m0);
  188.     r0[6] = s * (r0[6] - r1[6] * m0);
  189.     r0[7] = s * (r0[7] - r1[7] * m0);
  190.  
  191.     to[0][0] = r0[4];        /* copy results back        */
  192.     to[0][1] = r0[5];
  193.     to[0][2] = r0[6];
  194.     to[0][3] = r0[7];
  195.     to[1][0] = r1[4];
  196.     to[1][1] = r1[5];
  197.     to[1][2] = r1[6];
  198.     to[1][3] = r1[7];
  199.     to[2][0] = r2[4];
  200.     to[2][1] = r2[5];
  201.     to[2][2] = r2[6];
  202.     to[2][3] = r2[7];
  203.     to[3][0] = r3[4];
  204.     to[3][1] = r3[5];
  205.     to[3][2] = r3[6];
  206.     to[3][3] = r3[7];
  207.     return;
  208.  
  209. singular:
  210.     fprintf(stderr,"invertmat: singular matrix\n");
  211.     return;
  212. }
  213.